#include <iostream>
#include <memory>
#include <opencv2/highgui.hpp>
#include <opencv2/core.hpp>

#include <opencv2/imgproc.hpp>
#include <fstream>
#include "feature.h"
#include "frame.h"
#include "config.h"

#include "frontend.h"
#include <glog/logging.h>

using namespace std;
using namespace cv;
using namespace myslam;

struct yolo_data
{
    float cls;
    float x;
    float y;
    float w;
    float h;

};
cv::Mat get_mask(const Frame::Ptr& frame, const std::string& file);

int main() {




    std::string  config_file_path_ = "/home/zqk/桌面/slambook2/ch13/bin/config/default.yaml";


    if (!Config::SetParameterFile(config_file_path_)) {
        //判断一下这个config_file_path_是否存在，同时将配置文件赋给Config类中的cv::FileStorage file_,便于对文件操作
        return false;
    }

    cout<<Config::Get<std::string>("dataset_dir")<<endl;

    int64 t1, t2;




    // 1. 读取图片
    const cv::Mat image1 = cv::imread("/home/zqk/桌面/phone_test/IMG_0944.JPG", 0); //Load as grayscale
    const cv::Mat image2 = cv::imread("/home/zqk/桌面/phone_test/IMG_0945.JPG", 0); //Load as grayscale
    const cv::Mat image3 = cv::imread("/home/zqk/桌面/phone_test/IMG_0946.JPG", 0); //Load as grayscale
    const cv::Mat image4 = cv::imread("/home/zqk/桌面/phone_test/IMG_0949.JPG", 0); //2个missing head + 1个 dent
    const cv::Mat image5 = cv::imread("/home/zqk/桌面/phone_test/IMG_0948.JPG", 0); //只有上面的missing head
    const cv::Mat image6 = cv::imread("/home/zqk/桌面/phone_test/IMG_0950.JPG", 0); //只有下面的missing head

    //yolo检测的mask文件
    std::string file1_name = "/home/zqk/桌面/phone_test/IMG_0944.JPG.txt";
    std::string file2_name = "/home/zqk/桌面/phone_test/IMG_0945.JPG.txt";
    std::string file3_name = "/home/zqk/桌面/phone_test/IMG_0946.JPG.txt";
    std::string file4_name = "/home/zqk/桌面/phone_test/IMG_0949.JPG.txt";
    std::string file5_name = "/home/zqk/桌面/phone_test/IMG_0948.JPG.txt";
    std::string file6_name = "/home/zqk/桌面/phone_test/IMG_0950.JPG.txt";

    //新建帧1
    auto frame1 = Frame::CreateFrame();
    frame1->left_img_ = image1;
    frame1->masks = get_mask(frame1, file1_name);
    //新建帧2
    auto frame2 = Frame::CreateFrame();
    frame2->left_img_ = image2;
    frame2->masks = get_mask(frame2, file2_name);
    //新建帧3
    auto frame3 = Frame::CreateFrame();
    frame3->left_img_ = image3;
    frame3->masks = get_mask(frame3, file3_name);
    //新建帧4
    auto frame4 = Frame::CreateFrame();
    frame4->left_img_ = image4;
    frame4->masks = get_mask(frame4, file4_name);
    //新建帧5
    auto frame5 = Frame::CreateFrame();
    frame5->left_img_ = image5;
    frame5->masks = get_mask(frame5, file5_name);
    //新建帧6
    auto frame6 = Frame::CreateFrame();
    frame6->left_img_ = image6;
    frame6->masks = get_mask(frame6, file6_name);


    //新建前端
    Frontend::Ptr frontend_ = std::make_shared<Frontend>();

    //前端插入1帧
    if (!frontend_->AddFrame(frame1)) {
        cout << "第一帧加入失败" << endl;
    }


    if (!frontend_->AddFrame(frame2)) {
        cout << "第2帧加入失败" << endl;
    }


/*    if (!frontend_->AddFrame(frame3)) {
        cout << "第3帧加入失败" << endl;
    }

    if (!frontend_->AddFrame(frame4)) {
        cout << "第4帧加入失败" << endl;
    }

    if (!frontend_->AddFrame(frame5)) {
        cout << "第5帧加入失败" << endl;
    }
    if (!frontend_->AddFrame(frame6)) {
        cout << "第6帧加入失败" << endl;
    }*/


    return 0;
}



cv::Mat get_mask(const Frame::Ptr& frame, const std::string& file_name)
{

    std::fstream file;//创建文件流对象
    file.open(file_name);


    vector< struct yolo_data> image_data(2);


    //将txt文件数据写入到Data矩阵中
    //file里有2行代表mask
    for (int i = 0; i < 2; i++)
    {

        file >> image_data.at(i).cls;
        file >> image_data.at(i).x;
        file >> image_data.at(i).y;
        file >> image_data.at(i).w;
        file >> image_data.at(i).h;

        image_data.at(i).x -= image_data.at(i).w / 2;
        image_data.at(i).y -= image_data.at(i).h / 2;

        image_data.at(i).x *= frame->left_img_.cols;
        image_data.at(i).y *= frame->left_img_.rows;
        image_data.at(i).w *= frame->left_img_.cols;
        image_data.at(i).h *= frame->left_img_.rows;

    }



    for (auto& i : image_data)
    {
        cout << "i.cls  " << i.cls << endl;
        cout << "i.x  " << i.x << endl;
        cout << "i.y  " << i.y << endl;
        cout << "i.w  " << i.w << endl;
        cout << "i.h  " << i.h << endl;
    }



    cv::Mat mask(frame->left_img_.size(), CV_8UC1);
    {

        Rect2f temp(image_data.at(0).x, image_data.at(0).y, image_data.at(0).w, image_data.at(0).h);
        cv::rectangle(mask, temp, 10, cv::FILLED);
        Rect2f temp2(image_data.at(1).x, image_data.at(1).y, image_data.at(1).w, image_data.at(1).h);
        cv::rectangle(mask, temp2, 10, cv::FILLED);

    }

    return mask;


}


//#include <iostream>
//#include <opencv2/highgui.hpp>
//#include <opencv2/core.hpp>
//#include <opencv2/xfeatures2d.hpp>
//#include <opencv2/imgproc.hpp>
//#include <fstream>
//#include "feature.h"
//#include "frame.h"
//#include "mappoint.h"
//#include "frontend.h"
//#include <glog/logging.h>
//
//using namespace std;
//using namespace cv;
//using namespace myslam;
//
//struct yolo_data
//{
//    float cls;
//    float x;
//    float y;
//    float w;
//    float h;
//
//};
//cv::Mat get_mask(Frame::Ptr frame, std::string file);
//
//int main() {
//
//    
//    int64 t1, t2;
//    double tkpt, tdes, tmatch_bf, tmatch_knn;
//
//    // 1. 读取图片
//    const cv::Mat image1 = cv::imread("E:/BaiduNetdiskDownload/phone_test/IMG_0944.JPG", 0); //Load as grayscale
//    const cv::Mat image2 = cv::imread("E:/BaiduNetdiskDownload/phone_test/IMG_0945.JPG", 0); //Load as grayscale
//
//    //yolo检测的mask文件
//    std::string file1_name = "E:/BaiduNetdiskDownload/phone_test/IMG_0944.JPG.txt";
//    std::string file2_name = "E:/BaiduNetdiskDownload/phone_test/IMG_0945.JPG.txt";
//
//    //新建帧1
//    auto frame1 = Frame::CreateFrame();
//    frame1->left_img_ = image1;
//    frame1->masks = get_mask(frame1, file1_name);
//    //新建帧2
//    auto frame2 = Frame::CreateFrame();
//    frame2->left_img_ = image2;
//    frame2->masks = get_mask(frame2, file2_name);
//
//
//    //新建前端
//    Frontend::Ptr frontend_ = myslam::Frontend::Ptr(new Frontend);
//
//    //前端插入1帧
//    if (frontend_->AddFrame(frame1)) {
//
//    }
//    else {
//        cout << "第一帧加入失败" << endl;
//    }
//
//    if (frontend_->AddFrame(frame2)) {
//
//    }
//    else {
//        cout << "第2帧加入失败" << endl;
//    }
//
//
//    std::vector<cv::KeyPoint> keypoints1;
//    std::vector<cv::KeyPoint> keypoints2;
//
//
//    cv::Ptr<cv::SIFT> sift = cv::SIFT::create();
//    // 2. 计算特征点
//    t1 = cv::getTickCount();
//    //sift->detect(image1, keypoints1, frame1->masks);
//    t2 = cv::getTickCount();
//    tkpt = 1000.0 * (t2 - t1) / cv::getTickFrequency();
//    //sift->detect(image2, keypoints2, frame2->masks);
//
//
//    for (auto& f : frontend_->last_frame_->features_left_) {
//        keypoints1.push_back(f->position_);
//
//    }
//    for (auto& f : frontend_->current_frame_->features_left_) {
//        keypoints2.push_back(f->position_);
//
//    }
//
//
//    //for (auto& f : frame1->features_left_) {
//    //    keypoints1.push_back(f->position_);
//
//    //}
//    //for (auto& f : frame2->features_left_) {
//    //    keypoints2.push_back(f->position_);
//
//    //}
//
//    // 3. 计算特征描述符
//    cv::Mat descriptors1, descriptors2;
//    //t1 = cv::getTickCount();
//    //sift->compute(image1, keypoints1, descriptors1);
//    //t2 = cv::getTickCount();
//    //tdes = 1000.0 * (t2 - t1) / cv::getTickFrequency();
//    //sift->compute(image2, keypoints2, descriptors2);
//
//    descriptors1 = frontend_->last_frame_->descriptors;
//    descriptors2 = frontend_->current_frame_->descriptors;
//
//    // 4. 特征匹配
//    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create(cv::DescriptorMatcher::FLANNBASED);
//    // cv::BFMatcher matcher(cv::NORM_L2);
//
//    // (1) 直接暴力匹配
//    std::vector<cv::DMatch> matches;
//    t1 = cv::getTickCount();
//    matcher->match(descriptors1, descriptors2, matches);
//    t2 = cv::getTickCount();
//    tmatch_bf = 1000.0 * (t2 - t1) / cv::getTickFrequency();
//    // 画匹配图
//    cv::Mat img_matches_bf;
//    cv::drawMatches(image1, keypoints1, image2, keypoints2, matches, img_matches_bf);
//    namedWindow("bf_matches", 2);
//    cv::imshow("bf_matches", img_matches_bf);
//    cv::imwrite("bf_matches.jpg", img_matches_bf);
//
//    // (2) KNN-NNDR匹配法
//    std::vector<std::vector<cv::DMatch> > knn_matches;
//    const float ratio_thresh = 0.5f;
//    std::vector<cv::DMatch> good_matches;
//    t1 = cv::getTickCount();
//    matcher->knnMatch(descriptors1, descriptors2, knn_matches, 2);
//    for (auto& knn_matche : knn_matches) {
//        if (knn_matche[0].distance < ratio_thresh * knn_matche[1].distance) {
//            good_matches.push_back(knn_matche[0]);
//        }
//    }
//    t2 = cv::getTickCount();
//    tmatch_knn = 1000.0 * (t2 - t1) / cv::getTickFrequency();
//
//    // 画匹配图
//    cv::Mat img_matches_knn;
//    cv::drawMatches(image1, keypoints1, image2, keypoints2, good_matches, img_matches_knn);
//    namedWindow("knn_matches", 2);
//    cv::imshow("knn_matches", img_matches_knn);
//    cv::imwrite("knn_matches.jpg", img_matches_knn);
//
//
//    //    // (2) radius匹配法
//    std::vector<std::vector<cv::DMatch> > radiu_matches;
//    //检测半径
//    int radius = 100;
//
//    matcher->radiusMatch(descriptors1, descriptors2, radiu_matches, radius);
//
//
//
//    // 画匹配图
//    cv::Mat img_matches_radiu;
//    cv::drawMatches(image1, keypoints1, image2, keypoints2, radiu_matches, img_matches_radiu);
//    namedWindow("img_matches_radiu", 2);
//    cv::imshow("img_matches_radiu", img_matches_radiu);
//    cv::imwrite("img_matches_radiu.jpg", img_matches_radiu);
//
//
//
//    cv::waitKey(100);
//
//
//
//    cv::Mat output;
//    cv::drawKeypoints(image1, keypoints1, output);
//    cv::imwrite("sift_image1_keypoints.jpg", output);
//    cv::drawKeypoints(image2, keypoints2, output);
//    cv::imwrite("sift_image2_keypoints.jpg", output);
//
//
//    std::cout << "图1特征点检测耗时(ms)：" << tkpt << std::endl;
//    std::cout << "图1特征描述符耗时(ms)：" << tdes << std::endl;
//    std::cout << "BF特征匹配耗时(ms)：" << tmatch_bf << std::endl;
//    std::cout << "KNN-NNDR特征匹配耗时(ms)：" << tmatch_knn << std::endl;
//    return 0;
//}
//
//
//
//cv::Mat get_mask(Frame::Ptr frame, std::string file_name)
//{
//
//    std::fstream file;//创建文件流对象
//    file.open(file_name);
//
//
//    vector< struct yolo_data> image_data(2);
//
//
//    //将txt文件数据写入到Data矩阵中
//    //file里有2行代表mask
//    for (int i = 0; i < 2; i++)
//    {
//
//        file >> image_data.at(i).cls;
//        file >> image_data.at(i).x;
//        file >> image_data.at(i).y;
//        file >> image_data.at(i).w;
//        file >> image_data.at(i).h;
//
//        image_data.at(i).x -= image_data.at(i).w / 2;
//        image_data.at(i).y -= image_data.at(i).h / 2;
//
//        image_data.at(i).x *= frame->left_img_.cols;
//        image_data.at(i).y *= frame->left_img_.rows;
//        image_data.at(i).w *= frame->left_img_.cols;
//        image_data.at(i).h *= frame->left_img_.rows;
//
//    }
//
//
//
//    for (auto &i : image_data)
//    {
//        cout << "i.cls  " << i.cls << endl;
//        cout << "i.x  " << i.x << endl;
//        cout << "i.y  " << i.y << endl;
//        cout << "i.w  " << i.w << endl;
//        cout << "i.h  " << i.h << endl;
//    }
//
//
//
//    cv::Mat mask(frame->left_img_.size(), CV_8UC1);
//    {
//
//        Rect2f temp(image_data.at(0).x, image_data.at(0).y, image_data.at(0).w, image_data.at(0).h);
//        cv::rectangle(mask, temp, 10, cv::FILLED);
//        Rect2f temp2(image_data.at(1).x, image_data.at(1).y, image_data.at(1).w, image_data.at(1).h);
//        cv::rectangle(mask, temp2, 10, cv::FILLED);
//
//    }
//
//    return mask;
//
//
//}